#include "wheel_module.hpp"
#define SPEED_MAX 3096
#define SPEED_TIAO_MAX 2500
int wheel_module::conclude(void)
{
     int16_t temp16;
    float pid_p,pid_d,pid_i;
    float tempf,ptev_ave;
    pid_d=5.0;
    pid_p=1.0;
    pid_i=1.0;
    if(speed_module_set>SPEED_MAX)speed_module_set=SPEED_MAX;
    if(speed_module_set<-SPEED_MAX)speed_module_set=-SPEED_MAX;
    speed_set_L=-speed_module_set;
    speed_set_R= speed_module_set;
    temp16=(angle_set-angle_cur); ///原先是/2
    if(temp16>2000)temp16=2000;
    if(temp16<-2000)temp16=-2000;
    // prev_err[0]=last_err;
    // for(int i=0;i<4;i++)prev_err[i+1]=prev_err[i];
    // ptev_ave=0;
    // for(int i=0;i<5;i++)ptev_ave+=prev_err[i]/5.0;
    // last_err=cur_err;
    // cur_err=(float)temp16;
    // tempf=(pid_p*(cur_err-last_err)+pid_i*cur_err+pid_d*(ptev_ave))/8.4;
    // if(tempf>2000.0)tempf=2000.0;
    // if(tempf<-2000.0)tempf=-2000.0;
    cur_err=(float)temp16;
    prev_err[0]=cur_err;
     for(int i=0;i<19;i++)prev_err[i+1]=prev_err[i];
     ptev_ave=0;
     for(int i=0;i<20;i++)ptev_ave+=prev_err[i]/25.0;
    tempf=ptev_ave;
    if(tempf>5000.0)tempf=5000.0;
    if(tempf<-5000.0)tempf=-5000.0;
    temp16=tempf;
    if((speed_module_set>(-50))&(speed_module_set<50))  //150
    {
        if((temp16>(-160))&&(temp16<160))
          temp16=0;
    }
    speed_set_L+=temp16;
    speed_set_R+=temp16;
 
   
    return 0;
}

void wheel_module::set_module_speed(int sp)
{
    speed_module_set=sp;
}

float wheel_module::convert_speed_to_line(int sp)
{

}

int wheel_module::convert_line_to_speed(float lin)
{

}
